#include "util.h"


/* Function prototypes */

static inline void merge_m(data_t * __restrict A, int p, int q, int r) ;
static inline void copy_m(data_t * __restrict source, data_t * __restrict dest, int n) ;

/* Function definitions */

/* Basic merge sort */
void sort_m(data_t * A, int p, int r) 
{
	assert (A) ;
	if (r - p <= 49)
	{
	    isort(A+p,A+r);
	}
	else
	{
	    int q = (p + r) / 2 ;
	    sort_m(A, p, q) ;
	    sort_m(A, q + 1, r);
	    merge_m(A, p, q, r) ;
	}	
}

/* A merge routine. Merges the sub-arrays A [p..q] and A [q + 1..r].
 * Uses two arrays 'left' and 'right' in the merge operation.
 */
static inline void merge_m(data_t * __restrict A, int p, int q, int r) 
{ 
	assert(A) ;
	assert(p <= q) ;
	assert((q + 1) <= r) ;
	int n1 = q - p + 1;

	data_t * __restrict left = 0;
	data_t * __restrict right = A + q; 

	mem_alloc(&left, n1 + 1) ;
       if (left == NULL)
       {
       	mem_free(&left) ;
       	return ;
       }
	
	copy_m(A + p, left, n1) ;
	left [n1] = UINT_MAX ;

	data_t * __restrict rp = A + r ;
	data_t * __restrict Ap = A + p ;

	while(Ap <= rp)
	{
	    data_t cmp = *left <= *right ;
	    data_t min = *right ^ ((*left ^ *right) & (-cmp)) ;
	    *(Ap++) = min ;
	    left += cmp ;
	    right += !cmp ;
	}

	// Unwind left to pre-loop state so it can be properly freed:
	left -= n1 ;
	
	mem_free(&left) ;
}

static inline void copy_m(data_t * __restrict source, data_t * __restrict dest, int n)                              
{                                                                             
        assert (dest) ;                                                      
        assert (source) ;                                                     
	// Upper-bound of the destination to copy to:
	data_t * __restrict top = dest + n ;	
                                                               
	for ( ; dest < top ; dest++)
	{                                                 
	    *(dest) = *(source++) ;
	}
}
